#include "RL_FPV_MP.h"

void FPV::begin(HardwareSerial *serial, int rx, int tx){
  rpin = rx;
  tpin = tx;
  _serialH = serial;
  _serialH->begin(19200,rx,tx);

}

uint8_t FPV::FpvReadAll(void)
{

//当串口缓冲区大于等于一帧的长度时
  while (_serialH->available() >= 8) {
    uint8_t ubuffer[8];
    uint8_t verify_byte;
     //从串口缓冲读取1个字节但不删除
    uint8_t frame_header = _serialH->peek();
     //当获取的数据是包头(0x66)时
    if (frame_header == 0x66) {
       //从串口缓冲区读取8字节
       _serialH->readBytes(ubuffer, 8);
        if (ubuffer[7] == 0x99){
       verify_byte=ubuffer[1]^ubuffer[2]^ubuffer[3]^ubuffer[4]^ubuffer[5]&0xff;
       if(verify_byte==ubuffer[6]){

         for(int i=0;i<5;i++)
         fpvdata[i]=ubuffer[i+1];
      // Serial.println("11111");
          return 1;
         }
        }
    } else {
      _serialH->read();  //从串口缓冲读取1个字节并删除
       return 0;
     }
  }
  
   //return fpvubuffer;
}

uint8_t FPV::FpvReadByte(uint8_t index) {
  if(index<4) return fpvdata[index];
  else if(index==4) return (fpvdata[4]&0x01?1:0);
  else if(index==5) return (fpvdata[4]&0x02?1:0);
  else if(index==6) return (fpvdata[4]&0x04?1:0);
  else if(index==7) return (fpvdata[4]&0x10?1:0);
  else if(index==8) return (fpvdata[4]&0x80?1:0);
}
